#include "Mahony.h"
#include "MPU6050.h"
#include <math.h>

// 误差积分项
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;

// 采样频率倒数
#define SAMPLE_FREQ 100.0f
#define INV_SAMPLE_FREQ 0.01f  // 1.0/SAMPLE_FREQ

// Mahony算法更新
// 新的函数接口，参数顺序更合理，且支持可选的dt参数
void MahonyAHRSupdate(float q[4], float gx, float gy, float gz, float ax, float ay, float az, float dt) {
    // 如果dt为0，则使用默认值
    if (dt == 0.0f) {
        dt = INV_SAMPLE_FREQ;
    }
    float recipNorm;
    float halfvx, halfvy, halfvz;
    float halfex, halfey, halfez;
    float qa, qb, qc;

    // 加速度计归一化
    recipNorm = 1.0f / sqrtf(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    // 估计重力方向和叉积
    halfvx = q[1] * q[3] - q[0] * q[2];
    halfvy = q[0] * q[1] + q[2] * q[3];
    halfvz = q[0] * q[0] - 0.5f + q[3] * q[3];
    
    // 误差计算 - 叉积表示估计方向与测量方向的差异
    halfex = (ay * halfvz - az * halfvy);
    halfey = (az * halfvx - ax * halfvz);
    halfez = (ax * halfvy - ay * halfvx);
      // PI控制器应用于陀螺仪
    if(TWO_KI > 0.0f) {
        // 积分误差
        integralFBx += TWO_KI * halfex * dt;
        integralFBy += TWO_KI * halfey * dt;
        integralFBz += TWO_KI * halfez * dt;
        
        // 将积分误差应用到陀螺仪数据
        gx += integralFBx;
        gy += integralFBy;
        gz += integralFBz;
    }
    
    // 应用比例部分
    gx += TWO_KP * halfex;
    gy += TWO_KP * halfey;
    gz += TWO_KP * halfez;
    
    // 四元数微分方程的数值积分
    gx *= 0.5f * dt;
    gy *= 0.5f * dt;
    gz *= 0.5f * dt;
    qa = q[0];
    qb = q[1];
    qc = q[2];
    q[0] += (-qb * gx - qc * gy - q[3] * gz);
    q[1] += (qa * gx + qc * gz - q[3] * gy);
    q[2] += (qa * gy - qb * gz + q[3] * gx);
    q[3] += (qa * gz + qb * gy - qc * gx);
    
    // 四元数归一化
    recipNorm = 1.0f / sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
    q[0] *= recipNorm;
    q[1] *= recipNorm;
    q[2] *= recipNorm;
    q[3] *= recipNorm;
}

// 四元数转欧拉角
void QuaternionToEuler(float *q, float *roll, float *pitch, float *yaw) {
    // 转换为弧度
    *roll = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 1.0f - 2.0f * (q[1] * q[1] + q[2] * q[2]));
    *pitch = asinf(2.0f * (q[0] * q[2] - q[3] * q[1]));
    *yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 1.0f - 2.0f * (q[2] * q[2] + q[3] * q[3]));
    
    // 转换为角度
    *roll *= 57.29578f;   // 180/pi
    *pitch *= 57.29578f;
    *yaw *= 57.29578f;
}


